////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////

#include "ExplorerEngine.hpp"
#include "TKMacros.h"

////////////////////////////////////////////////////////////////////////////////
// Namespaces
////////////////////////////////////////////////////////////////////////////////

namespace SC {

////////////////////////////////////////////////////////////////////////////////
// Implementation
////////////////////////////////////////////////////////////////////////////////

using namespace std;
using namespace Types;

////////////////////////////////////////////////////////////////////////////////
// Public functions
////////////////////////////////////////////////////////////////////////////////

const unsigned int ExplorerEngine::AddBody() {
	this->bodies.push_back(LEBody<LEB_P,LEB_D>());
	return this->bodies.size()-1;
}

const bool ExplorerEngine::UpdateBodies(float & wp_bodies) {
	bool updated = false;
	wp_bodies = 0.f;
	// Compute the location displacements due to forces
	for (unsigned int i=0; i<this->bodies.size(); ++i) {
		LEBody<LEB_P,LEB_D> & body = this->bodies[i];
		if (body.neighbours != NULL)
			for (unsigned int j=0; j<body.neighbours->size(); ++j)
				if ((*body.neighbours)[j] != i)
					body.disp += this->repulsion(body,this->bodies[(*body.neighbours)[j]]);
/*
		// Eventually apply a drift
		if (this->drift_strength > 0.) {
			Vector2D<LE_PTYPE>	drift_vec = body.location - Point2D<LE_PTYPE>(0.,0.);
			drift_vec *= 1./drift_vec.Norm() - 1.;
			body.disp += drift_vec * this->drift_strength;
		}*/
///*
	}
	// For each body
	for (unsigned int i=0; i<this->bodies.size(); ++i) {
		LEBody<LEB_P,LEB_D> & body = this->bodies[i];
//*/
		// Backup point location
		Point2D<LEB_P> tmp_loc(body.location);

		// Check motion threshold
		if (body.disp.Norm() >= this->threshold)
			// Update point location
			body.location += body.disp;

		// Check boundaries with containing space (ball of radius 1)
		Vector2D<LEB_P> v(-body.location.x, -body.location.y);
		const double vnorm = v.Norm();
		if (vnorm > 1. - body.size - this->margin) {
			v *= (vnorm - (1. - body.size - this->margin))/vnorm;
			body.location += v;
		}

		// Check location change
		if (tmp_loc != body.location) updated = true;
		// If no change, increase the well placed bodies amount
		else ++wp_bodies;

		// Reset displacement
		body.disp.x = body.disp.y = 0.;		
		
	}
	// Amount of well placed bodies
	wp_bodies /= (float)this->bodies.size();
	// Returns true if an update happened
	return updated;
}

void ExplorerEngine::SetBodyScale(const double value) {
	double factor = value/this->body_scale;
	for (unsigned int i=0; i<this->bodies.size(); ++i)
		this->bodies[i].size *= factor;
	this->body_scale = value;
}

void ExplorerEngine::ApplyAttraction(
	const vector<unsigned int> & bodies_idx,
	const bool att_parent
) {

	// Compute the isobarycentre of the bodies
	Point2D<LEB_P> barycentre(0.,0.);
	for (unsigned int b=0; b<bodies_idx.size(); ++b)
		barycentre += (this->bodies[bodies_idx[b]].location - Point2D<LEB_P>(0.,0.))/static_cast<float>(bodies_idx.size());

	// If there is no attraction to parent (boundaries)
	if (!att_parent)
		// Attract all bodies towards the barycentre with the given strength
		for (unsigned int b=0; b<bodies_idx.size(); ++b)
			this->bodies[bodies_idx[b]].disp += (barycentre - this->bodies[bodies_idx[b]].location) * this->att_strength;

	// Otherwise, if parent attraction
	else {
		// Get the projection of the barycentre on the parent perimeter from the parent centre
		Vector2D<LEB_P> tmp = (barycentre - Point2D<LEB_P>(0.,0.));
		tmp /= tmp.Norm();
		Point2D<LEB_P> attraction = Point2D<LEB_P>(0.,0.) + tmp;
		// Attract the bodies towards this parent point
		for (unsigned int b=0; b<bodies_idx.size(); ++b)
			this->bodies[bodies_idx[b]].disp += (attraction - this->bodies[bodies_idx[b]].location) * this->att_strength;
	}

}

////////////////////////////////////////////////////////////////////////////////
// Protected functions
////////////////////////////////////////////////////////////////////////////////

//#define DA 0.//(M_PI/3.)
inline const Vector2D<LEB_P> ExplorerEngine::repulsion(const LEBody<LEB_P,LEB_D> & body, const LEBody<LEB_P,LEB_D> & fellow) const {
	Vector2D<LEB_P> v = body.location - fellow.location;
	double vnorm = v.Norm();
	if (vnorm == 0.) v = Vector2D<LEB_P>((rand()&1?1.f:-1.f)*this->threshold, (rand()&1?1.f:-1.f)*this->threshold);
	if (vnorm < body.size + fellow.size + this->margin) {
		/*
		v /= norm;
		double angle;
		if (v.GetY() > 0.) angle = acos(v.GetX());
		else angle = -acos(v.GetX());
		angle += (frand()*DA*2.) - DA;
		v.X() = cos(angle);
		v.Y() = sin(angle);
		v *= norm;
		return v * exp(body.size + fellow.size + this->margin - norm) * frand() * this->step;*/
		return v * exp(body.size + fellow.size + this->margin - vnorm) * this->rep_strength;
	}
	else return v * 0.;
}

////////////////////////////////////////////////////////////////////////////////
// Closing Namespaces
////////////////////////////////////////////////////////////////////////////////

}

////////////////////////////////////////////////////////////////////////////////
